
#include "Filt1.h"

Filt1::Filt1(float Tf_, float Te_)
{
  
    if(Te_ <= 0.001)
    {
     Te_  = 0.001; 
    }
    
    if(Tf_ <= Te_)
    {
        Tf_ = Te_;
    }
    
    Tf = Tf_;
    Te = Te_;
    S = 0.0;
    E = 0.0; 
    S_m1 = 0.0;
    E_m1 = 0.0;
    

}

Filt1::Filt1()
{
   
     
    
    Tf = 0.001;
    Te = 0.001;
    S = 0.0;
    E = 0.0; 
    S_m1 = 0.0;
    E_m1 = 0.0;

       
}

Filt1::~Filt1()
{
    
}

void Filt1::init(float init_s)
{
    E_m1 = init_s;
    E = init_s;
    S= init_s;
    S_m1 = init_s;

}

void Filt1::SetTf(float Tf_)
{
    if(Tf_ <= Te)
    {
        Tf_ = Te;
    } 
    Tf = Tf_;
    
}

void Filt1::SetTe(float Te_)
{
    if(Te_ <= 0.001)
    {
        Te_ = 0.001;
    } 
    Te = Te_;
}

void Filt1::Config(float Tf_, float Te_)
{
    if(Te_ <= 0.001)
    {
     Te_  = 0.001; 
    }
    
    if(Tf_ <= Te_)
    {
        Tf_ = Te_;
    }
    Tf = Tf_;
    Te = Te_;
    S = 0.0;
    E = 0.0; 
    S_m1 = 0.0;
    E_m1 = 0.0;
       
}

float Filt1::GetS()
{
  return S;
}

float Filt1::filt(float e_)
{
    
     
    E = e_;
    
    S =(float) (2.0*Tf-Te)/(Te+2.0*Tf)*S_m1 + Te/(Te+2.0*Tf)*(E+E_m1);

    E_m1  =  E;
    S_m1  =  S;
    
    return S;
}
